#include <ros/ros.h>
#include <nav_msgs/Odometry.h>
#include <tf/transform_broadcaster.h>
#include <geometry_msgs/Twist.h>
#include <sensor_msgs/Imu.h>

class OdometryNode {
public:
    OdometryNode() : x_(0.0), y_(0.0), th_(0.0), vx_(0.0), vy_(0.0), vth_(0.0) {
        // 订阅速度命令和IMU数据
        vel_sub_ = nh_.subscribe("cmd_vel", 10, &OdometryNode::velCallback, this);
        imu_sub_ = nh_.subscribe("imu", 10, &OdometryNode::imuCallback, this);

        // 发布里程计数据
        odom_pub_ = nh_.advertise<nav_msgs::Odometry>("odom", 50);

        // 创建tf广播器
        odom_broadcaster_ = std::make_shared<tf::TransformBroadcaster>();

        // 设置定时器，定期更新里程计
        timer_ = nh_.createTimer(ros::Duration(0.1), &OdometryNode::updateOdom, this);
    }

    void velCallback(const geometry_msgs::Twist::ConstPtr& msg) {
        vx_ = msg->linear.x;
        vy_ = msg->linear.y;
        vth_ = msg->angular.z;
    }

    void imuCallback(const sensor_msgs::Imu::ConstPtr& msg) {
        // 更新IMU数据
        th_ += msg->angular_velocity.z * 0.1;  // 假设IMU数据的频率为10Hz
    }

    void updateOdom(const ros::TimerEvent& event) {
        // 计算新的位姿
        double dt = 0.1;  // 时间间隔
        double delta_x = (vx_ * cos(th_) - vy_ * sin(th_)) * dt;
        double delta_y = (vx_ * sin(th_) + vy_ * cos(th_)) * dt;
        double delta_th = vth_ * dt;

        x_ += delta_x;
        y_ += delta_y;
        th_ += delta_th;

        // 创建并发布里程计消息
        nav_msgs::Odometry odom;
        odom.header.stamp = ros::Time::now();
        odom.header.frame_id = "odom";
        odom.child_frame_id = "base_footprint";

        // 设置位姿
        odom.pose.pose.position.x = x_;
        odom.pose.pose.position.y = y_;
        odom.pose.pose.position.z = 0.0;
        odom.pose.pose.orientation = tf::createQuaternionMsgFromYaw(th_);

        // 设置速度
        odom.twist.twist.linear.x = vx_;
        odom.twist.twist.linear.y = vy_;
        odom.twist.twist.angular.z = vth_;

        // 发布里程计消息
        odom_pub_.publish(odom);

        // 广播tf变换
        geometry_msgs::TransformStamped odom_trans;
        odom_trans.header.stamp = odom.header.stamp;
        odom_trans.header.frame_id = "odom";
        odom_trans.child_frame_id = "base_footprint";

        odom_trans.transform.translation.x = x_;
        odom_trans.transform.translation.y = y_;
        odom_trans.transform.translation.z = 0.0;
        odom_trans.transform.rotation = odom.pose.pose.orientation;

        odom_broadcaster_->sendTransform(odom_trans);
    }

private:
    ros::NodeHandle nh_;
    ros::Subscriber vel_sub_;
    ros::Subscriber imu_sub_;
    ros::Publisher odom_pub_;
    std::shared_ptr<tf::TransformBroadcaster> odom_broadcaster_;
    ros::Timer timer_;

    double x_, y_, th_;
    double vx_, vy_, vth_;
};

int main(int argc, char** argv) {
    ros::init(argc, argv, "odometry_node");
    OdometryNode node;

    ros::spin();
    return 0;
}
